#include <stdio.h>
#include <string.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <driver/gpio.h>
#include "buttons.h"

#define BUTTON_UP_PIN        37
#define BUTTON_CENTER_PIN    38
#define BUTTON_DOWN_PIN      36

#define LONG_PRESS_STAGE1       8
#define LONG_PRESS_STAGE2       50
#define STAGE2_ACCELERATION     4

QueueHandle_t xButtonsEventQueue;

typedef struct {
    uint8_t pin;
    uint8_t pressEvent;
    uint8_t holdEvent;
    bool wasPressed;
    bool wasHeldDown;
    uint32_t counter;
    uint8_t stageCounter;
} sGPIOButton;

void buttons_init() {
    gpio_config_t io_conf;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = BIT64(BUTTON_UP_PIN) | BIT64(BUTTON_CENTER_PIN) | BIT64(BUTTON_DOWN_PIN);
    io_conf.pull_down_en = false;
    io_conf.pull_up_en = false;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    gpio_config(&io_conf);
}

void inline sendButtonEvent(uint8_t event) {
    xQueueSend(xButtonsEventQueue, (void *) &event, (TickType_t) 10);
}

uint8_t inline isSinglePressReleased(sGPIOButton *button, bool pressed) {
    return !button->wasHeldDown && button->wasPressed && !pressed;
}

void handleStateChangeOf(sGPIOButton *button) {
    bool pressed = gpio_get_level(button->pin) == 0;
    if (pressed && button->wasPressed) {
        if ((++button->counter > LONG_PRESS_STAGE2) ||
            (button->counter > LONG_PRESS_STAGE1 && ++button->stageCounter == STAGE2_ACCELERATION)) {
            button->stageCounter = 0;
            button->wasHeldDown = true;
            sendButtonEvent(button->holdEvent);
        }
    } else if (isSinglePressReleased(button, pressed)) {
        button->counter = button->stageCounter = 0;
        sendButtonEvent(button->pressEvent);
    }
    if (!pressed) button->wasHeldDown = false;
    button->wasPressed = pressed;
}

void buttons_task(void *arg) {
    xButtonsEventQueue = xQueueCreate(20, sizeof(uint8_t));
    if (xButtonsEventQueue == NULL) {
        vTaskDelete(NULL);
        return;
    }
    sGPIOButton buttonUp = {BUTTON_UP_PIN, ShortPress_Up, Hold_Up, false, false, 0, 0};
    sGPIOButton buttonCenter = {BUTTON_CENTER_PIN, ShortPress_Center, Hold_Center, false, false, 0, 0};
    sGPIOButton buttonDown = {BUTTON_DOWN_PIN, ShortPress_Down, Hold_Down, false, false, 0, 0};
    while (1) {
        handleStateChangeOf(&buttonUp);
        handleStateChangeOf(&buttonCenter);
        handleStateChangeOf(&buttonDown);
        vTaskDelay(100 / portTICK_RATE_MS);
    }
}


